#ifndef MAPPING_OCTOMAP_MULTI_SCAN_H
#define MAPPING_OCTOMAP_MULTI_SCAN_H
 
#include "mapping/ground_filter.h"
#include "mapping/map_saver.h"
#include "mapping/tools/tic_toc.h"
#include "mapping/show_path.h"
#include <cloud_pose_msg/CloudPose.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>

#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/GetOctomap.h>
#include <octomap_msgs/BoundingBoxQuery.h>
#include <octomap_msgs/conversions.h>

#include <octomap_ros/conversions.h>
#include <octomap/octomap.h>
#include <octomap/OcTreeKey.h>

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

#include <thread>
#include <vector>
#include <iostream>
#include <glog/logging.h>
#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>
#include <mutex>

class Gen_grid_map{
public:
    Gen_grid_map(ros::NodeHandle& nh_);//通过ros param 来读取参数 包括 res
    void cloud_callback(const cloud_pose_msg::CloudPoseConstPtr& msg_in);
    void save_2d_map();
private:
    void filtering(const pcl::PointCloud<pcl::PointXYZ>::Ptr& _cloud_in,pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_out);
    void ground_filtering(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in);
    void DEBUG_pub_cloud_in_map(cloud_pose_msg::CloudPoseConstPtr msg_in);
    void ray_tracing();
    void gen_2d_map();
    void handle_occupied_node(octomap::OcTree::iterator& it);
    void handle_free_node(octomap::OcTree::iterator& it);
    void update_2d_map(const octomap::OcTree::iterator& it, bool occupied);
    inline unsigned mapIdx(int i, int j) const {return grid_map.info.width * j + i;}
    inline unsigned mapIdx(const octomap::OcTreeKey& key) const {
        return mapIdx((key[0] - padded_min_key[0]) ,(key[1] - padded_min_key[1]) );}
private:
    std::mutex mtx;
    std::shared_ptr<Ground_filter> p_ground_filter;
    
    int key_frame_gap;
    int frame_count = 0;

    ros::NodeHandle nh;
    ros::Subscriber    sub_cloud;
    ros::Publisher     pub_grid_map;
    ros::Publisher     pub_ground_cloud;
    ros::Publisher     pub_non_ground_cloud;
    ros::Publisher     pub_full_cloud;
    // ros::ServiceServer map_service_server;

    octomap::OcTree* tree;
    octomap::point3d sensor_origin;
    octomap::OcTreeKey min_key,max_key;
    octomap::KeySet occupied_cells,free_cells;
    octomap::OcTreeKey padded_min_key;

    nav_msgs::OccupancyGrid grid_map;
    nav_msgs::OccupancyGrid final_grid_map;
    double res,radius_filter_th,z_max_th,distance_th,eps_angle;
    double prob_hit,prob_miss,clamping_th_min,clamping_th_max;
    unsigned int max_tree_depth;

    Eigen::Matrix4f current_frame_pose;

    pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud_in_map;
    pcl::PointCloud<pcl::PointXYZ>::Ptr non_ground_cloud_in_map;
    pcl::PointCloud<pcl::PointXYZ>::Ptr full_cloud_in_map;

    //path for 2d grid map
    std::string png_path;
    std::string yaml_path;
    std::shared_ptr<Map_saver> p_map_saver;

    bool do_ground_filtering = false;
};
#endif